%%%-------------------------------------------------------------------
%%% @author Administrator
%%% @copyright (C) 2020, <COMPANY>
%%% @doc
%%%  寻路算法的设计
%%% @end
%%% Created : 07. 4月 2020 20:33
%%%-------------------------------------------------------------------
-module(map_route).
-author("Administrator").
-include("common.hrl").

%% API
-export([get_route/3]).

-record(map_cell, {
	pos = {0, 0},
	key = 0,  %% 索引key
	base_type = 0,  %% 基础类型 MAP_TYPE_*
	type = 1,       %% 生成后类型 MAP_POS_TYPE_*
	val = 0         %% 预留
}).

%% 获取可走的路径
-spec get_route(Maps :: [#map{}], StartPos :: {integer(),integer()},EndPos :: {integer(),integer()}) ->[{integer(),integer()}].
get_route(Maps, StartPos, [EndPos]) ->
	?DEBUG("StartPos = ~p, EndPos = ~p", [StartPos, EndPos]),
	ClosedSet = sets:new(),
	OpenSet = sets:add_element(StartPos,sets:new()),
	FScore = dict:append(StartPos,h_score(StartPos,EndPos),dict:new()),
	GScore = dict:append(StartPos,0,dict:new()),
	Routes = dict:append(StartPos,none,dict:new()),
	case a_star_step(Maps, EndPos, ClosedSet, OpenSet, FScore, GScore, Routes) of
		failure ->
			false;
		Routes_1 ->
			RouteList = get_route_pos(Routes_1, EndPos),
			?DEBUG("route list = ~999999p",[lists:reverse(RouteList)])
	end.

get_route_pos(Routes, Node) ->
	case dict:fetch(Node, Routes) of
		[none] ->
			[Node];
		[Val] ->
			[Node|get_route_pos(Routes, Val)]
	end.

a_star_step(Maps, EndPos, CloseSet, OpenSet, FScore, GScore, Routes) ->
	case sets:size(OpenSet) of
		0 ->
			failure;
		_ ->
			OpenSetList = sets:to_list(OpenSet),
			case lists:member(EndPos,OpenSetList) of
				true ->
					Routes;
				false ->
					BestNode = best_step(OpenSetList, FScore, none, infinity),
					OpenSet_1 = sets:del_element(BestNode, OpenSet),
					CloseSet_1 = sets:add_element(BestNode, CloseSet),
					NeighbourNodes = neighbour_nodes(BestNode),
					{OpenSet_2, FScore_1, GScore_1, Routes_1} = scan(Maps, BestNode, EndPos, NeighbourNodes, OpenSet_1, CloseSet_1, FScore, GScore, Routes),
					a_star_step(Maps, EndPos, CloseSet_1, OpenSet_2, FScore_1, GScore_1, Routes_1)
			end
	end.

%% 获取最佳节点周围的节点
neighbour_nodes({X, Y}) ->
	CheckPos = [{X-1, Y},{X+1,Y},{X,Y-1},{X,Y+1}],
	CheckPos.

%% 算法介绍
%% 1.如果在关闭sets中，则滤过
%% 2.如果已经在opensets中，则需要判断，G值的大小
%% 3.判断地图的可走行问题
scan(_Maps, _StartPos, _EndPos, [], OpenSets, _CloseSets, FScore, GScore, Routes) ->
	{OpenSets, FScore, GScore, Routes};
scan(Maps, StartPos, EndPos, [TestPos|TCheckPos], OpenSets, CloseSets, FScore, GScore, Routes) ->
	case sets:is_element(TestPos,CloseSets) of
		true ->
			scan(Maps, StartPos, EndPos, TCheckPos, OpenSets, CloseSets, FScore, GScore, Routes);
		false ->
			[StartG] = dict:fetch(StartPos, GScore),
			TrialG = StartG + g_dist_between(StartPos,TestPos),
			case sets:is_element(TestPos, OpenSets) of
				true ->
					[TestG] = dict:fetch(TestPos, GScore),
					if
						TrialG < TestG -> % 此处发现，通过start->testpos路径比之前的testpos路径短，所以重置
							{FScore_1, GScore_1, Routes_1 } = update_pos_info(StartPos, TestPos, EndPos, FScore, GScore, Routes, TrialG),
							scan(Maps, StartPos, EndPos, TCheckPos, OpenSets, CloseSets, FScore_1, GScore_1, Routes_1);
						true ->
%%						此处还不能将TestPos从OpenSets删除，
%% 						应为可能这条路可能有障碍，需要重新选择路径，所以需要计算进去
							scan(Maps, StartPos, EndPos, TCheckPos, OpenSets, CloseSets, FScore, GScore, Routes)
					end;
				false ->
					case check_map(Maps, TestPos) of
						ok ->
							OpenSets_1 = sets:add_element(TestPos, OpenSets),
							{FScore_1, GScore_1, Routes_1} = update_pos_info(StartPos, TestPos, EndPos, FScore, GScore, Routes, TrialG),
							scan(Maps, StartPos, EndPos, TCheckPos, OpenSets_1, CloseSets, FScore_1, GScore_1, Routes_1);
						false ->
							scan(Maps, StartPos, EndPos, TCheckPos, OpenSets, CloseSets, FScore, GScore, Routes)
					end
			end
	end.

update_pos_info(StartPos, TestPos, EndPos, FScore, GScore, Routes, GTestVal) ->
	KeyF = dict:is_key(TestPos, FScore),
	KeyG = dict:is_key(TestPos, GScore),
	KeyR = dict:is_key(TestPos, Routes),

	case {KeyF, KeyG, KeyR} of
		{true, _, _} ->
			update_pos_info(StartPos, TestPos, EndPos, dict:erase(TestPos, FScore), GScore, Routes, GTestVal);
		{_, true, _} ->
			update_pos_info(StartPos, TestPos, EndPos, FScore, dict:erase(TestPos, GScore), Routes, GTestVal);
		{_, _, true} ->
			update_pos_info(StartPos, TestPos, EndPos, FScore, GScore, dict:erase(TestPos, Routes), GTestVal);
		_ ->
			FScore_1 = dict:append(TestPos, GTestVal + h_score(TestPos, EndPos), FScore),
			GScore_1 = dict:append(TestPos, GTestVal, GScore),
			Routes_1 = dict:append(TestPos, StartPos, Routes),
			{FScore_1, GScore_1, Routes_1}
	end.

check_map(Maps, TestPos) ->
	case lists:keyfind(TestPos, #map_cell.pos, Maps#map.map_cell) of
		false ->
			false;
		MapCell ->
			if
				MapCell#map_cell.type =< 0 ->
					false;
				true ->
					case MapCell#map_cell.type of
						1 ->
							ok;
						_ ->
							false
					end
			end
	end.

%% 获取最佳节点
best_step([H|TOpenSets], FScore, none, _) ->
	[V] = dict:fetch(H,FScore),
	best_step(TOpenSets, FScore, H, V);
best_step([], _FScore,Best,_BestValue) ->
	Best;
best_step([H|TOpenSets], FScore, Best, BestValue) ->
	[V] = dict:fetch(H,FScore),
	case V < BestValue of
		true ->
			best_step(TOpenSets, FScore, H, V);
		false ->
			best_step(TOpenSets, FScore, Best, BestValue)
	end.

%% 获取g值的，即当前节点到测试可走节点的G值
g_dist_between({CurX,CurY}, {TestX,TestY}) ->
	abs(CurX-TestX) + abs(CurY-TestY).

%% 获取h启发函数的取值，采用曼哈顿距离 d(i,j)=|X1-X2|+|Y1-Y2|
h_score({CurX,CurY}, {EndX,EndY}) ->
	abs(CurX-EndX) + abs(CurY-EndY).
